#include "LcBasic.h"

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/video.hpp"
#include <opencv2/gpu/gpu.hpp>

namespace lc
{	

	double normrnd()
	{		
		double X;
		static double V1, V2, S;
		static int phase = 0;

		if ( phase == 0 ) {
			do {
				double U1 = (double)rand() / RAND_MAX;
				double U2 = (double)rand() / RAND_MAX;
		         
				V1 = 2 * U1 - 1;
				V2 = 2 * U2 - 1;
				S = V1 * V1 + V2 * V2;
			} while(S >= 1 || S == 0);
		     
			X = V1 * sqrt(-2 * log(S) / S);
		} else
			X = V2 * sqrt(-2 * log(S) / S);
		     
		phase = 1 - phase;

		return X;
	}

	Mat normrnd( Size size)
	{
		Mat ans = Mat::zeros( size , 5);

		int _sz = ans.rows * ans.cols;

		float* _data = (float*) ans.data;

		for(int i = 0; i< _sz; i++, _data++)
			*_data = float( normrnd() );

		return ans;
	}


	Mat randMatrix( Size size)
	{
		Mat ans = Mat::zeros( size , 5);

		int _sz = ans.rows * ans.cols;

		float* _data = (float*) ans.data;

		for(int i = 0; i< _sz; i++, _data++)
			*_data = float( rand()/float(RAND_MAX) );

		return ans;
	}

	void swapMatrix( Mat & src, Mat & dst, Mat & src_lab, Mat & dst_lab)
	{
		int n = (int) src.rows;

		if( src.rows != src_lab.rows )
		{
			cout << "warning! size unmatched while swaping" << endl;
			exit(1);
		}
		if( dst.rows != src.rows ) src.copyTo(dst);
		if( dst_lab.rows != src.rows ) src_lab.copyTo(dst_lab);

		Mat temp; src.row(0).copyTo(temp);
		Mat temp_lab; src_lab.row(0).copyTo(temp_lab);

		for(int i = 0;i<n;i++)
		{
			int id1 = i;//rand() % n;
			int id2 = rand() % n;
			
			src.row(id1).copyTo(temp);
			src.row(id2).copyTo(dst.row(id1));
			temp.copyTo(dst.row(id2));

			src_lab.row(id1).copyTo(temp_lab);
			src_lab.row(id2).copyTo(dst_lab.row(id1));

			//if(i%10000==0 || temp_lab.at<float>) cout << id1 << " " << id2 << " " << temp_lab.at<float>(0,0) << " | ";

			temp_lab.copyTo(dst_lab.row(id2));
		}
	}

	void swapMatrix( Mat & src, Mat & dst)
	{
		int n = (int) src.rows;
		if( dst.rows != src.rows ) src.copyTo(dst);

		Mat temp; src.row(0).copyTo(temp);

		for(int i = 0;i<n;i++)
		{
			int id1 = i;
			int id2 = rand() % n;
			src.row(id1).copyTo(temp);
			src.row(id2).copyTo(dst.row(id1));
			temp.copyTo(dst.row(id2));
		}	
	}

	void LcMat2Bin( const char * file_name, Mat & src)
	{
		FILE * fin;
		//fopen_s(&fin,file_name,"wb");
		fin = fopen( file_name,"wb");

		int _rows = src.rows;
		int _cols = src.cols;

		fwrite( &_rows , sizeof(int),1, fin);

		fwrite( &_cols , sizeof(int),1, fin);

		src.convertTo(src,5);

		for(int i = 0;i< _rows ;i++)
			for(int j = 0; j< _cols ; j++)
			{
				fwrite( &(src.at<float>(i,j)) , sizeof(float),1, fin);
			}



		fclose(fin);
	}

	void LcBin2Mat( const char * file_name, Mat & ans)
	{
		FILE * fid;
		//fopen_s(&fid, file_name ,"rb");
		
		fid = fopen(file_name ,"rb");

		int _rows;
		int _cols;

		fread( &_rows , sizeof(int),1, fid);
		//cout << "row is " << _rows << endl;

		fread( &_cols , sizeof(int),1, fid);
		//cout << "col is " << _cols << endl;

		ans = Mat::zeros( _rows, _cols, 5);

		for(int i = 0;i< _rows ;i++)
			for(int j = 0; j< _cols ; j++)
				fread( &(ans.at<float>(i,j)) , sizeof(float),1, fid);

		//cout << "Ans is" << endl << ans << endl;

		fclose(fid);
	}


	namespace _Affine2Homography
	{
		Point2f transbyAffine( Mat & A, Point2f po)
		{
			Mat v = Mat(po);
			v.push_back(1.0f);
			v = A*v;
			Vec3f s(v);
			return Point2f(s[0],s[1]);
		}

		void para2Homo( Mat & para, Mat & H)
		{
			int k = 0;
			for(int i = 0;i<3;i++)
				for(int j = 0;j<3;j++)
				{
					if(k==8) break;
					H.at<double>(i,j) = para.at<double>(k,0); k++;
				}
			H.at<double>(2,2) = 1.0;
		}

		void Homo2para( Mat & para, Mat & H)
		{
			if(para.rows!=8) para.create(8,1,CV_64F);
			Mat A;
			if(para.type()!=H.type())
			{
				H.copyTo(A); A.convertTo(A,para.type());
			}
			else A = H;

			int k = 0;
			for(int i = 0;i<3;i++)
				for(int j = 0;j<3;j++)
				{
					if(k==8) break;
					para.at<double>(k,0) = A.at<double>(i,j); k++;
				}
		}

		void calcJac( Mat & para, Mat & jac,Mat & mpo0, Mat & mpo1)
		{
			int n = mpo0.rows;

			jac.create( n*2 +8 , 8 , CV_64F);

			Mat err1;Mat err2;

			for(int j = 0;j< 8;j++)
			{
				const double step = 1e-4;

				double & val = para.at<double>(j , 0);

				val += step;

				calcErr( para,err1,mpo0,mpo1);

				val -= (2*step);

				calcErr( para,err2,mpo0,mpo1);

				calcDeriv(err1, err2, 2 * step, jac.col(j));		
			}
		}

		void calcErr( Mat & para, Mat & err,Mat & mpo0, Mat & mpo1)
		{
			Mat H = Mat::zeros(3,3,CV_64F);  para2Homo(para, H);
			Mat res_mpo1 = mpo0 * H.t();
			int n = mpo0.rows;
			int _type = res_mpo1.type();

			if( _type == CV_64F)
				for(int i = 0;i<n;i++) res_mpo1.row(i) = res_mpo1.row(i)/ res_mpo1.at<double>(i,2);
			else
				for(int i = 0;i<n;i++) res_mpo1.row(i) = res_mpo1.row(i)/ res_mpo1.at<float>(i,2);

			Mat temp0; cv::absdiff( res_mpo1.col(0),mpo1.col(0),temp0);
			Mat temp1; cv::absdiff( res_mpo1.col(1),mpo1.col(1),temp1);
			Mat temp2 = cv::abs(para) * double(n);

			err.create(n*2+8,1, _type );

			temp0.copyTo(err.rowRange(0, n)); temp1.copyTo(err.rowRange(n, n+n));
			temp2.copyTo(err.rowRange(n+n,n+n+8));
		}

		void calcDeriv(const Mat &err1, const Mat &err2, double h, Mat res)
		{
			for (int i = 0; i < err1.rows; ++i)
				res.at<double>(i, 0) = (err2.at<double>(i, 0) - err1.at<double>(i, 0)) / h;
		}
	};

	Mat Affine2Homography( Mat & Affine,vector<Point2f> & po0, vector<Point2f> & po1)
	{
		int _type = CV_64F; // Affine.type();
		Mat H = Mat::zeros(3,3,_type); Mat para = Mat::zeros(8,1, _type);

		if(po0.size()!=po1.size())
		{
			cout << "warning size unmatch in Affine2Homography" << endl;
			return H;
		}

		_Affine2Homography::Homo2para( para, Affine );
		para.at<double>(6,0) = double((rand()%100-50)/1000);
		para.at<double>(7,0) = double((rand()%100-50)/1000);

		int point_n = (int) po0.size();
		Mat mpo0 = Mat::ones(point_n,3,5),mpo1 = Mat::ones(point_n,3,5);
		mpo0.convertTo(mpo0, _type); mpo1.convertTo(mpo1, _type);
		points2mat( po1,mpo1); points2mat( po0,mpo0);

			CvLevMarq solver(8,2*point_n+8);

			Mat err,jac;

			CvMat temp_para = para;
			cvCopy( &temp_para,solver.param);

			int iter = 0;
			for(;;)
			{
				const CvMat * _param = 0;
				CvMat * p_jac = 0;
				CvMat * p_err = 0;

				bool proceed = solver.update(_param ,p_jac,p_err);

				if (!proceed || !p_err) break;

				cvCopy(_param, &temp_para);

				if(p_err)
				{
					_Affine2Homography::calcErr( para,err,mpo0,mpo1);

					CvMat tmp = err;
					cvCopy(&tmp, p_err);
				}
				
				if(p_jac)
				{

					_Affine2Homography::calcJac( para,jac ,mpo0,mpo1);

					//cout << jac.rows << " == " << 2*point_n << " && " << jac.cols << " == 8" << endl;

					CvMat tmp = jac;
					cvCopy(&tmp, p_jac);

					
				}

				iter++;
			}

		_Affine2Homography::para2Homo(para,H);
		H.convertTo( H, Affine.type() );
		return H;
	}

	void Homo2D( Point2f & src,Point2f & dst, Mat H)
	{
		Mat vec = Mat::ones(3,1,5);
		vec.at<float>(0,0) = src.x;
		vec.at<float>(1,0) = src.y;
		vec = H * vec;;
		/*if(H.type() == 5) vec = H * vec;
		else{ vec.convertTo(vec,H.type()); vec = H * vec; vec.convertTo(vec,5);}*/
		dst.x = vec.at<float>(0,0);
		dst.y = vec.at<float>(1,0);

	}

	void points2mat(std::vector<Point2f> & po, Mat & mpo)
	{
		int n = (int) po.size();

		if(mpo.rows!= n) mpo = Mat::zeros(n,2,5);

		for(int i = 0;i<n;i++)
		{
			mpo.at<float>(i,0) = po[i].x;
			mpo.at<float>(i,1) = po[i].y;
		}
	}

	Mat Points2Homography(
	   Mat & po1,
	   Mat & po2)
	{
		Mat H = Mat::zeros(3,3,5);

		H.at<float>(2,2) = 1;

		int n = (int) po1.rows;
		
		if(n!= (int) po2.rows)
		{
			cout << "warining! size unmatch between two points vector in IDT" << endl;
			return H;
		}

		Mat b = Mat::zeros(n*2,1,5);
		Mat A = Mat::zeros(n*2,8,5);
		Mat x;

		{
			for(int i = 0, j = 0;
				i<n;
				i++)
			{
				float & x = po1.at<float>(i,0);
				float & y = po1.at<float>(i,1);
				float & u = po2.at<float>(i,0);
				float & v = po2.at<float>(i,1);

				A.at<float>(j,0) = x;
				A.at<float>(j,1) = y;
				A.at<float>(j,2) = 1;
				A.at<float>(j,6) = -x*u;
				A.at<float>(j,7) = -x*v;
				b.at<float>(j,0) = u;
				j++;

				A.at<float>(j,3) = x;
				A.at<float>(j,4) = y;
				A.at<float>(j,5) = 1;
				A.at<float>(j,6) = -y*u;
				A.at<float>(j,7) = -y*v;
				b.at<float>(j,0) = v;
				j++;		

			}

			Mat P = (A.t() * A);

			x = P.inv()*(A.t() * b);
		}

		{
			int k = 0;
			for(int j = 0;j<3;j++)
				for(int i = 0;i<3;i++,k++)
				{
					if(k==8) break;
					H.at<float>(j,i) = x.at<float>(k,0);
				}
		}
			
		return H;
	}

	Mat Points2Affine(Mat & po1,Mat & po2)
	{
		Mat H = Mat::zeros(3,3,5);

		H.at<float>(2,2) = 1;

		int n = (int) po1.rows;
		
		if(n!= (int) po2.rows)
		{
			cout << "warining! size unmatch between two points vector in IDT" << endl;
			return H;
		}

		Mat b = Mat::zeros(n*2,1,5);
		Mat A = Mat::zeros(n*2,6,5);
		Mat x;

		{
			for(int i = 0, j = 0;
				i<n;
				i++)
			{
				float & x = po1.at<float>(i,0);
				float & y = po1.at<float>(i,1);
				float & u = po2.at<float>(i,0);
				float & v = po2.at<float>(i,1);

				A.at<float>(j,0) = x;
				A.at<float>(j,1) = y;
				A.at<float>(j,2) = 1;
				b.at<float>(j,0) = u;
				j++;

				A.at<float>(j,3) = x;
				A.at<float>(j,4) = y;
				A.at<float>(j,5) = 1;
				b.at<float>(j,0) = v;
				j++;		

			}

			Mat P = (A.t() * A);

			x = P.inv()*(A.t() * b);
		}

		{
			int k = 0;
			for(int j = 0;j<2;j++)
				for(int i = 0;i<3;i++,k++)
				{
					H.at<float>(j,i) = x.at<float>(k,0);
				}
		}
			
		return H;
	}


	void randperm( int n, std::vector<int> & ord)
	{
		ord.resize(n);

		for(int i = 0;i<n;i++) ord[i] = i;

		for(int i = 0;i<n;i++)
		{
			int j = rand()%n;

			int k = ord[i];
			ord[i] = ord[j];
			ord[j] = k;
		}
	}

	Mat Points2Homography(
	   std::vector<Point2f> & po1,
	   std::vector<Point2f> & po2)
	{
		Mat H = Mat::zeros(3,3,5);

		H.at<float>(2,2) = 1;

		int n = (int) po1.size();
		
		if(n!= (int) po2.size())
		{
			cout << "warining! size unmatch between two points vector in IDT" << endl;
			return H;
		}

		Mat b = Mat::zeros(n*2,1,5);
		Mat A = Mat::zeros(n*2,8,5);
		Mat x;

		{
			for(int i = 0, j = 0;
				i<n;
				i++)
			{
				float & x = po1[i].x;
				float & y = po1[i].y;
				float & u = po2[i].x;
				float & v = po2[i].y;

				A.at<float>(j,0) = x;
				A.at<float>(j,1) = y;
				A.at<float>(j,2) = 1;
				A.at<float>(j,6) = -x*u;
				A.at<float>(j,7) = -x*v;
				b.at<float>(j,0) = u;
				j++;

				A.at<float>(j,3) = x;
				A.at<float>(j,4) = y;
				A.at<float>(j,5) = 1;
				A.at<float>(j,6) = -x*u;
				A.at<float>(j,7) = -x*v;
				b.at<float>(j,0) = v;
				j++;		

			}

			Mat P = (A.t() * A);

			x = P.inv()*(A.t() * b);
		}

		{
			int k = 0;
			for(int j = 0;j<3;j++)
				for(int i = 0;i<3;i++,k++)
				{
					if(k==8) break;
					H.at<float>(j,i) = x.at<float>(k,0);
				}
		}
			
		return H;
	}

	Mat Points2Affine(
	   std::vector<Point2f> & po1,
	   std::vector<Point2f> & po2)
	{
		Mat H = Mat::zeros(3,3,5);

		H.at<float>(2,2) = 1;

		int n = (int) po1.size();
		
		if(n!= (int) po2.size())
		{
			cout << "warining! size unmatch between two points vector in IDT" << endl;
			return H;
		}

		Mat b = Mat::zeros(n*2,1,5);
		Mat A = Mat::zeros(n*2,6,5);
		Mat x;

		{
			for(int i = 0, j = 0;
				i<n;
				i++)
			{
				float & x = po1[i].x;
				float & y = po1[i].y;
				float & u = po2[i].x;
				float & v = po2[i].y;

				A.at<float>(j,0) = x;
				A.at<float>(j,1) = y;
				A.at<float>(j,2) = 1;
				b.at<float>(j,0) = u;
				j++;

				A.at<float>(j,3) = x;
				A.at<float>(j,4) = y;
				A.at<float>(j,5) = 1;
				b.at<float>(j,0) = v;
				j++;		

			}

			Mat P = (A.t() * A);

			x = P.inv()*(A.t() * b);
		}

		{
			int k = 0;
			for(int j = 0;j<2;j++)
				for(int i = 0;i<3;i++,k++)
				{
					H.at<float>(j,i) = x.at<float>(k,0);
				}
		}
			
		return H;
	}

	void diffPoints( std::vector<Point2f> & src , std::vector<Point2f> & dsc)
	{
		int n = (int) src.size();

		if(n<2) return;
		
		dsc.resize(n);

		for(int i = 0;i<n-1;i++) dsc[i] = src[i+1] - src[i];

		dsc.resize(n-1);

	}
	Mat points2Mat( std::vector< Point2f > & points,bool if_norm)
	{
		int n = (int) points.size();

		Mat ans = Mat::zeros( 1,2*n,5);

		if( if_norm )
		{
			float sumx = 0,sumy = 0;
			for(int i = 0;i<n;i++)
			{
				ans.at<float>(2*i+1) = points[i].x;
				sumx += points[i].x;
				ans.at<float>(2*i+2) = points[i].y;
				sumy += points[i].y;
			}
		}
		else
		{
			for(int i = 0;i<n;i++)
			{
				ans.at<float>(2*i+1) = points[i].x;
				ans.at<float>(2*i+2) = points[i].y;
			}
		}

		return ans;
	}

	Mat getBase( Mat & data, Mat & weight, int m ,bool if_norm )
	{
		int n = data.rows;
		int dim = data.cols;

		if(weight.rows!=n || weight.cols!=dim)
		{
			printf("warning!, size of weight and data unmatch, return the ans without weight\n");
			return getBase( data , m , if_norm);
		}

		Mat A = Mat::ones( dim,dim,5) * 1e-5;
		Mat W = Mat::ones( dim,dim,5) * 1e-5;

		for(int i = 0;i<n;i++)
		{
			Mat temp = data.row(i);
			Mat tempw = weight.row(i);

			if(if_norm)
			{
				temp.convertTo( temp, CV_32F);
				temp = temp - reduceMat( temp, CV_REDUCE_AVG);
			}
			A = A + temp.t() * temp;
			W = W + tempw.t() * tempw;
		}

		cv::divide(A,W,A);

		Mat eigenvalues, eigenvectors;
		cv::eigen( A, eigenvalues,eigenvectors);

		Mat ans;

		eigenvectors.rowRange( 0, m ).copyTo( ans);
		
		return ans;
	}

	Mat getBase( Mat & data, int m, bool if_norm)
	{
		int n = data.rows;
		int dim = data.cols;

		Mat A = Mat::zeros( dim,dim,5);

		for(int i = 0;i<n;i++)
		{
			Mat temp = data.row(i);

			if(if_norm)
			{
				temp.convertTo( temp, CV_32F);
				temp = temp - reduceMat( temp, CV_REDUCE_AVG);
			}
			A = A + temp.t() * temp;
		}

		Mat eigenvalues, eigenvectors;
		cv::eigen( A, eigenvalues,eigenvectors);

		Mat ans;

		eigenvectors.rowRange( 0, m ).copyTo( ans);
		
		return ans;
	}

	Mat getProjMatrix( Mat & base)
	{
		Mat ans;
		Mat temp = base * base.t();

		ans = base.t() * temp.inv() * base;

		return ans;
	}


	float getSAD( Mat & A, Mat & B)
	{
		if(A.rows!=B.rows || A.cols!=B.cols)
		{
			cout << "waring size unmatch in getSSD" << endl;
			return 0;
		}
		float ans = 0;
		for(int i = 0;i<A.rows;i++) for(int j = 0;j<A.cols;j++) ans += fabs(float(A.at<float>(i,j)));
		return ans;
	}

	float reduceMat(
		Mat & mat,
		int rtype)
	{
		Mat temp_col,temp;
		cv::reduce(mat ,temp_col,0,rtype);
		cv::reduce(temp_col,temp,1,rtype);

		temp.convertTo(temp, CV_32F);

		return temp.at<float>(0);
	}

	Mat colorshow( const std::string  winname, Mat & src)
	{
		Mat temp;

		colormap(src,temp);

		imshow( winname, temp);

		return temp;
	}

		

	void filterMat( Mat & src, Mat & dsc, std::vector<int > & id)
	{
		int n = (int) id.size();

		dsc = Mat::zeros( n,n, src.type());

		vector<int>::iterator iter = id.begin();
		vector<int>::iterator iter_end = id.end();

		for(int i = 0 ; iter<iter_end;i++,iter++)
		{
			vector<int>::iterator iterc = id.begin();
			vector<int>::iterator iterc_end = id.end();

			for(int j = 0 ; iterc<iterc_end;j++,iterc++)
			{
				dsc.at<float>(i,j) = src.at<float>(*iter,*iterc);
			}

		}
	}

	bool LcSortcompare( const LcSortElement & a, const LcSortElement & b)
	{
		return a.element < b.element;
	}
	
	void filterCol(Mat & src, Mat & dsc, std::vector<int> id)
	{
		Mat temp_src = src.t();

		Mat temp_dsc;

		filterRow( temp_src, temp_dsc, id);

		dsc = temp_dsc.t();
	}

	void filterRow(Mat & src, Mat & dst, std::vector<int> id)
	{
		int _rows = src.rows;
		int _cols = src.cols;
		int _n_rows = (int) id.size();
		int _type = src.type();

		dst = Mat::zeros( _n_rows, _cols, _type);

		for(int i = 0;i<_n_rows;i++)
		{
			int _rid = id[i];

			if(_rid >= _rows)
			{
				cout << " warning refree index out of src matrix " << 
					_rid << " >= " << _rows << endl;
				continue;
			}
			src.row(_rid).copyTo(dst.row(i));
		}
	}

	void colormap(Mat src, Mat &dst, int do_norm)
	{
		
		

		double minVal,maxVal;
		
		
		Mat im = src;

		im.convertTo(im,5);

		minMaxLoc(im,&minVal,&maxVal,NULL,NULL);

		cout << "colormap minmax: " << minVal << " " << maxVal << endl;

		if(do_norm) im = (src-minVal)/(maxVal-minVal);		// normalization [0 to 1]
		
		
		if(1){														// my HSV mapping with masking
			
			Mat mask;	
			mask = Mat::ones(src.size(),CV_8UC1)*255.0;	
			
			compare(im,FLT_MIN,mask,CMP_GT);						// one color values greater than X	
			//threshold(src,src,FLT_MIN,0,THRESH_TOZERO);			// 		
			
			Mat U8;
			im.convertTo(U8,CV_8UC1,255,0);
			
			Mat I3[3],hsv;
			I3[0] = U8 * 0.85;
			I3[1] = mask;
			I3[2] = mask;
			merge(I3,3,hsv);
			cvtColor(hsv,dst,CV_HSV2RGB_FULL);
		}
		else{
			/*
			 COLORMAP_AUTUMN = 0,		// works
			 COLORMAP_BONE = 1,			// works
			 COLORMAP_JET = 2,
			 COLORMAP_WINTER = 3,
			 COLORMAP_RAINBOW = 4,		// not implemented
			 COLORMAP_OCEAN = 5,
			 COLORMAP_SUMMER = 6,
			 COLORMAP_SPRING = 7,
			 COLORMAP_COOL = 8,
			 COLORMAP_HSV = 9,			// broken
			 COLORMAP_PINK = 10,
			 COLORMAP_HOT = 11,
			 COLORMAP_MKPJ1 = 12,		// works
			 COLORMAP_MKPJ2 = 13
			 */
			Mat U8;
			im.convertTo(U8,CV_8UC1,255,0);
			applyColorMap(U8,dst,13);
		}
		
	}

	void gscatter( Mat & img, std::vector<Point2f> points, std::vector<int> id)
	{
		int max = 0;

		int n = (int) id.size();

		for(int i = 0;i<n;i++) if(id[i]>max) max = id[i];

		max++;

		Mat mymap = Mat::zeros( 1, max , CV_8U );

		for(int i = 0;i<max;i++) mymap.at<unsigned char>(i) = i*255/max;

		applyColorMap( mymap,mymap, COLORMAP_JET);

		for(int i = 0;i<n;i++)
		{
			int _v = id[i];
			Vec3b _color = mymap.at<Vec3b>(_v);
			circle(img, points[i] ,1,CV_RGB( _color[0],_color[1],_color[2]),2,CV_AA);
		}

	}

	void appendColorDescriptors(Mat src,vector<Point> & keypts,Mat &desc, string space)
	{

		int code;
		if(space=="RGB") code = CV_BGR2RGB;
		else if(space=="HSV") code = CV_BGR2HSV_FULL;
		else if(space=="LAB") code = CV_BGR2Lab;
		
		Mat color;
		cvtColor(src,color,code);

		for(int k = 0 ; k < (int) keypts.size() ; k++)
		{
			int x = (int)floor(keypts[k].x+.4);
			int y = (int)floor(keypts[k].y+.4);
			{
				desc.at<float>(k,0) = color.at<Vec3b>(y, x) (0) / 255.f;
				desc.at<float>(k,1) = color.at<Vec3b>(y, x) (1) / 255.f;
				desc.at<float>(k,2) = color.at<Vec3b>(y, x) (2) / 255.f;
			}
		}

		
	}
	

	float cvCrossProduct(Point & p1,Point & p2,Point & p3)
	{
		float x1 = (float)p2.x - p1.x;
		float x2 = (float)p3.x - p2.x;
		float y1 = (float)p2.y - p1.y;
		float y2 = (float)p3.y - p2.y;

		return (x1*y2-x2*y1);
	}

	int getClockWise(vector<Point>  & co , float eps )
	{
		int n = (int) co.size();

		int ans = 0;
		for(int i = 0;i < n; i++)
		{
			int i2 = (i+1)%n;
			int i3 = (i+2)%n;

			float v = cvCrossProduct(co[i],co[i2],co[i3]);

			if(v>0) ans++;
			else ans--;
			continue;

			if(v>eps) ans++;
			else if(v<-eps) ans--;
			
		}

		return ans;

		if(ans>0) return 1;
		else if(ans<0) return -1;
		else return 0;

	}


	int tryimread(const string filename,Mat & src,int flag)
	{
		src = imread(filename,flag);
		if( !src.data ) return 0;
		else return 1;
	}

	Mat gray2alpha(Mat graymask , Mat src)
	{

		Mat mask;

		Mat dsp;

		if(graymask.type()==CV_8UC3)
		{
			cvtColor(graymask,mask,CV_BGR2GRAY);
		}
		else
		{
			mask = graymask;
		}

		cvtColor( mask , dsp, CV_GRAY2BGR);

		addWeighted(dsp, 0.5, src, 0.5, 0, dsp);

		return dsp;
	}


	

	void argsort( vector<float> & data, vector<int> & id)
	{
		int n = (int) data.size();
		id.resize(n);

		vector<LcSortElement> a; a.resize(n);
		for(int i = 0;i<n;i++){a[i].id = i; a[i].element = data[i];}

		//std::sort( a.begin(), a.end(), LcSortElement::compare);
		std::sort( a.begin(), a.end(), LcSortcompare);

		for(int i = 0;i<n;i++){ id[i] = a[i].id;}
	}

	void getChannel( Mat & frm, Mat & frm_gray , char channel_code)
	{
		int id = 0;
		switch( channel_code )
		{
			case 'R': id = 0;break;
			case 'G': id = 1;break;
			case 'B': id = 2;break;
			case 'L': id = 3;break;
			case 'a': id = 4;break;
			case 'b': id = 5;break;
			case 'H': id = 6;break;
			case 'S': id = 7;break;
			case 'V': id = 8;break;
		}

		int code;
		if( id < 3) code = CV_BGR2RGB;
		else if( id < 6) code = CV_BGR2Lab;
		else code = CV_BGR2HSV_FULL;

		Mat color;
		cvtColor(frm,color,code);

		vector<Mat> channels;

		cv::split(color, channels);

		channels[id%3].copyTo( frm_gray );
	}
};

